import time
from Sentry3 import *
from mpython import *
from machine import UART


uart1 = UART(1, baudrate=57600, tx=Pin.P14, rx=Pin.P13)

Sentry3 = Sentry3(0x60)

brightness=9

# 自定义函数
def UART():
  global my_variable, i, start, angle
  Sentry3.begin(uart1)
def I2C():
  global my_variable, i, start, angle
  Sentry3.begin(i2c)
def Result_Display():
  global my_variable, i, start, angle
  # 利用A与B按键控制小车的启停
  if (start > 0):
    # Sentry3不主动返回检测识别结果，需要主控板发送指令进行读取。读取的流程：首先读取识别结果的数量，接收到指令后，Sentry3会刷新结果数据，如果结果数量不为零，那么主控再发送指令读取结果的相关信息。请务必按此流程构建程序。
    if ((Sentry3.GetValue(sentry3_vision_e.kVisionLine, sentry_obj_info_e.kStatus)) > 0):
      angle = (Sentry3.GetValue(sentry3_vision_e.kVisionLine,sentry_obj_info_e.kLabel,1))
      oled.DispChar((str("angle=") + str(angle)), 0, (2-1)*16, 1)
      if (45 > angle):
        Hard_Right()
      if ((80 > angle) and (angle > 45)):
        Right()
      if ((100 > angle) and (angle > 80)):
        Go_Straight()
      if ((135 > angle) and (angle > 100)):
        Left()
      if (angle > 135):
        Hard_Left()
      oled.show()
      time.sleep(0.2)
      oled.fill_rect(0, (2-1)*16, 128, 16, 0)
      oled.fill_rect(0, (3-1)*16, 128, 16, 0)
      oled.show()
def Go_Straight():
  global my_variable, i, start, angle
  oled.DispChar("Go Straight", 0, (3-1)*16, 1)
def Left():
  global my_variable, i, start, angle
  oled.DispChar("Left", 0, (3-1)*16, 1)
def Right():
  global my_variable, i, start, angle
  oled.DispChar("Right", 0, (3-1)*16, 1)
def Hard_Left():
  global my_variable, i, start, angle
  oled.DispChar("Hard Left", 0, (3-1)*16, 1)
def Hard_Right():
  global my_variable, i, start, angle
  oled.DispChar("Hard Right", 0, (3-1)*16, 1)

# 事件回调函数
def on_button_a_down(_):
  time.sleep_ms(10)
  if button_a.value() == 1: return
  global start
  start = 1
def on_button_b_down(_):
  time.sleep_ms(10)
  if button_b.value() == 1: return
  global start
  start = 0


button_a.irq(trigger=Pin.IRQ_FALLING, handler=on_button_a_down)
button_b.irq(trigger=Pin.IRQ_FALLING, handler=on_button_b_down)
angle = 0
start = 0
# 等待Sentry3初始化完毕，本延时不可省略，避免出现Sentry3尚未初始化完毕主控板已经开始发送指令的情况
time.sleep(2)
I2C()
# 算法的默认设置，如无需更改，则可以移除本代码块
Sentry3.VisionSetMode(sentry3_vision_e.kVisionLine, 1)
Sentry3.VisionBegin(sentry3_vision_e.kVisionLine)
oled.DispChar("Algo: Line", 0, (1-1)*16, 1)
oled.show()
while True:
  rgb[1] = (0*brightness//9, 0*brightness//9, 255*brightness//9)
  rgb.write()
  Result_Display()
  rgb[1] = (0*brightness//9, 0*brightness//9, 0*brightness//9)
  rgb.write()
  time.sleep(0.2)
